#include "wifibot/wifibot.h"

#define CTRL_POS 320
#define TEXT_START_Y 120
#define MIN_DIST_DIFF 0.01
#define DEF_DIST_CRITIC 1.0
#define DEF_DIST_STOP 0.5

namespace Lio {

// WifibotControlPanel - Class For The Application:
// FlatApplication - base class, provides all necessary windows routines
class WifibotControlPanel:public WifibotApplication
{
public:
	// Constructor Initialization:
	// 0.3 - wheel distance
	// 1 - max speed
	static double attenuation(double distLeft, double distRight, double distCritic, double distStop);
	static double avoid(double distLeft, double distRight);
protected:
	// critical range for speed attenuation
	double distCritic,distStop;
	double sl,sr,vlin,vrot;
	double reduct,kav,kat;

	// Window controls: Labels, TextBoxes, Scrolls and Buttons for the Main Window
	FlatLabel lLeft,lRight,lRot,lLin,lDistCritic,lDistStop;
	FlatText tbLeft,tbRight,tbRot,tbLin,tbDistCritic,tbDistStop;
	FlatScroll sLeft,sRight,sLin,sRot;
	FlatButton bControl,bCollision,bWheelCtrl,bSpeedCtrl,bAdd,bUp,bDown,bLeft,bRight,bAIAvoid;

	// robot - The object containg all necessary routines for communication with Wifibot
	Wifibotsc12 robot;

	Console console;
	void dump(FILE *file);

	// Application specific functions
	// create - Invoked when the application starts for window and user initializations
	// glCreate - Invoked for OpenGl initialization
	// glDestroy - Invoked for OpenGl termination
	// destroy - Invoked when terminating the application
	// reshape - Invoked when resizing the window
	// listen - Invoked when receiving a message
	// update - Main program loop body, place update code here
	// draw2D - Procedure for drawing in 2D
	// draw3D - Procedure for drawing in 3D
	void draw2D();
	void updateAction();
	void connectAction();
	BOOL listenAction(WORD id, WORD code, void *info);
	BOOL createAction();
} app;
double WifibotControlPanel::attenuation(double distLeft, double distRight, double distCritic, double distStop)
{
	return (distCritic>0&&distStop!=distCritic)?(min(distLeft,distRight)-distStop)/(distCritic-distStop):1.0;
}
double WifibotControlPanel::avoid(double distLeft, double distRight)
{
	return (absval(distLeft-distRight)>MIN_DIST_DIFF)?(distLeft-distRight)/(distLeft+distRight):0.0;
}
void WifibotControlPanel::updateAction()
{
	// Get the values from the Scroll Bars and send the data to the Wifibot:
	if(bWheelCtrl.checked)
	{
		sl=remap(sLeft.getRatio(),0.0,1.0,-1.0,1.0);
		sr=remap(sRight.getRatio(),0.0,1.0,-1.0,1.0);
	}
	else if(bSpeedCtrl.checked)
	{
		if((bUp.checked||keyboard.keys[VK_UP])&&!sLin.isActive())
		{
			sLin.increase();
		}
		else if((bDown.checked||keyboard.keys[VK_DOWN])&&!sLin.isActive())
		{
			sLin.decrease();
		}
		if((bLeft.checked||keyboard.keys[VK_LEFT])&&!sRot.isActive())
		{
			sRot.decrease();
		}
		else if((bRight.checked||keyboard.keys[VK_RIGHT])&&!sRot.isActive())
		{
			sRot.increase();
		}
		vrot=remap(sRot.getRatio(),0.0,1.0,1.0,-1.0);
		vlin=remap(sLin.getRatio(),0.0,1.0,-1.0,1.0);
		robot.SpeedToWheelControl(vrot,vlin,&sl,&sr);
	}
	kav=kat=0;
	if(bCollision.checked||bAIAvoid.checked)
	{
		robot.WheelToSpeedControl(sl,sr,&vrot,&vlin);
		if(bCollision.checked)
		{
			// Avoid collision by attenuation:
			kat=attenuation(robotSensors.DistLeft,robotSensors.DistRight,distCritic,distStop);
			vlin=min(vlin,kat);
		}
		if(bAIAvoid.checked)
		{
			// Avoid collision by rotation:
			kav=avoid(robotSensors.DistLeft,robotSensors.DistRight);
			vrot=maxabsval(vrot,kav);
		}
		robot.SpeedToWheelControl(vrot,vlin,&sl,&sr);
	}
	reduct=robot.WheelToComControl(sl,sr,&coml,&comr);
	// Update speeds in order to show them on the screen:
	sl*=reduct;
	sr*=reduct;
	robot.WheelToSpeedControl(sl,sr,&vrot,&vlin);
}
void WifibotControlPanel::connectAction()
{
	if(!bConnect.checked)
	{
		sLin.setRatio(0.5);
		sRot.setRatio(0.5);
		sLeft.setRatio(0.5);
		sRight.setRatio(0.5);
	}
}
BOOL WifibotControlPanel::createAction()		
{
	name=_T("Wifibot Control Panel");
	
	distCritic=DEF_DIST_CRITIC;
	distStop=DEF_DIST_STOP;

	bControl.text=_T("Control");
	bControl.copy(bConnect);
	bControl.stick(DIR_DOWN,0,10,&bFilter);
	bControl.checked = robot.control;

	bCollision.text=_T("Collision");
	bCollision.copy(bConnect);
	bCollision.stick(DIR_RIGHT,5,0);

	bAIAvoid.text=_T("Avoid");
	bAIAvoid.copy(bConnect);
	bAIAvoid.stick(DIR_RIGHT,5,0);

	lDistCritic.text=_T("Dist critic:");
	lDistCritic.fontColor=INT_BLACK;
	lDistCritic.margin.set(5,1);
	lDistCritic.adjustSize=TRUE;
	lDistCritic.stick(DIR_RIGHT,5,0);

	tbDistCritic.copy(tbPort);
	tbDistCritic.text=format.toString(distCritic);
	tbDistCritic.stick(DIR_RIGHT,0,0);
	tbDistCritic.size.x=60;

	lDistStop.text=_T("Dist stop:");
	lDistStop.copy(lDistCritic);
	lDistStop.stick(DIR_RIGHT,5,0);

	tbDistStop.copy(tbDistCritic);
	tbDistStop.text=format.toString(distStop);
	tbDistStop.stick(DIR_RIGHT,0,0);

	bWheelCtrl.text=_T("Left - Right Speed:");
	bWheelCtrl.copy(bConnect);
	bWheelCtrl.pressColor=INT_CYAN;
	bWheelCtrl.stick(DIR_RIGHT,20,0,&fgIR);
	bWheelCtrl.checked=FALSE;

	lLeft.text=_T(" left:");
	lLeft.copy(lDistCritic);
	lLeft.stick(DIR_DOWN,0,10,&bWheelCtrl);

	tbLeft.copy(tbIP);
	tbLeft.text="0.000";
	tbLeft.size.x=70;
	tbLeft.stick(DIR_RIGHT,5,0,&lLeft);

	sLeft.colorStart=INT_RED;
	sLeft.colorMiddle=INT_YELLOW;
	sLeft.colorEnd=INT_RED;
	sLeft.size.set(100,20);
	sLeft.borderLine=1;
	sLeft.setDirection(DIR_RIGHT);
	sLeft.stick(DIR_RIGHT,5,0,&tbLeft);
	sLeft.setRatio(0.5);
	sLeft.step=0.005;

	lRight.text=_T("right:");
	lRight.copy(lLeft);
	lRight.stick(DIR_DOWN,0,5,&lLeft);

	tbRight.copy(tbLeft);
	tbRight.text="0.000";
	tbRight.stick(DIR_RIGHT,5,0,&lRight);

	sRight.copy(sLeft);
	sRight.stick(DIR_RIGHT,5,0,&tbRight);
	sRight.setRatio(0.5);

	bSpeedCtrl.text=_T("Rot - Lin Speed:");
	bSpeedCtrl.copy(bWheelCtrl);
	bSpeedCtrl.stick(DIR_DOWN,0,100,&bWheelCtrl);
	bSpeedCtrl.checked=TRUE;

	bUp.text=_T("/\\");
	bUp.copy(bConnect);
	bUp.pressColor=INT_WGREEN;
	bUp.stick(DIR_DOWN,40,10,&bSpeedCtrl);
	bUp.size.set(40,20);
	bUp.checkable=FALSE;

	bDown.text=_T("\\/");
	bDown.copy(bUp);
	bDown.stick(DIR_DOWN,0,0,&bUp);

	bLeft.text=_T("<");
	bLeft.copy(bUp);
	bLeft.stick(DIR_LEFT,0,0,&bDown);

	bRight.text=_T(">");
	bRight.copy(bUp);
	bRight.stick(DIR_RIGHT,0,0,&bDown);

	sRot.copy(sLeft);
	sRot.stick(DIR_DOWN,0,5,&bLeft);
	sRot.size.set(120,20);
	sRot.setRatio(0.5);
	sRot.step=0.05;

	sLin.copy(sRot);
	sLin.stick(DIR_RIGHT,45,0,&bUp);
	sLin.size.set(20,65);
	sLin.setDirection(DIR_UP);

	lRot.text=_T("Rot:");
	lRot.copy(lLeft);
	lRot.stick(DIR_RIGHT,5,0,&sLin);

	tbRot.copy(tbLeft);
	tbRot.text="0.000";
	tbRot.stick(DIR_RIGHT,5,0,&lRot);

	lLin.text=_T("Lin:");
	lLin.copy(lLeft);
	lLin.stick(DIR_DOWN,0,5,&lRot);

	tbLin.copy(tbLeft);
	tbLin.text="0.000";
	tbLin.stick(DIR_RIGHT,5,0,&lLin);

	// Include the controls in the window
	add(&bControl);
	add(&bCollision);
	add(&bAIAvoid);
	add(&lDistCritic);
	add(&tbDistCritic);
	add(&lDistStop);
	add(&tbDistStop);
	

	add(&bWheelCtrl);
	add(&lLeft);
	add(&tbLeft);
	add(&sLeft);
	add(&lRight);
	add(&tbRight);
	add(&sRight);

	add(&bSpeedCtrl);
	add(&bUp);
	add(&bDown);
	add(&bLeft);
	add(&bRight);
	add(&sRot);
	add(&sLin);
	add(&lRot);
	add(&tbRot);
	add(&lLin);
	add(&tbLin);
	
	return TRUE;
}
BOOL WifibotControlPanel::listenAction(WORD id, WORD code, void *info)
{
	if(id==bControl.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			robot.control=bControl.checked;
			return TRUE;
		}
	}
	else if(id==tbLeft.id)
	{
		if(code==FLAT_ACTION_EDIT)
		{
			double r=remap(limit(format.toDouble(tbLeft.getText()),-1.0,1.0),-1.0,1.0,0.0,1.0);
			sLeft.setRatio(r);
			return TRUE;
		}
	}
	else if(id==sLeft.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			tbLeft.text=format.toString(remap(sLeft.getRatio(),0.0,1.0,-1.0,1.0));
			return TRUE;
		}
	}
	else if(id==tbRight.id)
	{
		if(code==FLAT_ACTION_EDIT)
		{
			double r=remap(limit(format.toDouble(tbRight.getText()),-1.0,1.0),-1.0,1.0,0.0,1.0);
			sRight.setRatio(r);
			return TRUE;
		}
	}
	else if(id==sRight.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			tbRight.text=format.toString(remap(sRight.getRatio(),0.0,1.0,-1.0,1.0));
			return TRUE;
		}
	}
	else if(id==bSpeedCtrl.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			bWheelCtrl.checked=FALSE;
			return TRUE;
		}
	}
	else if(id==bWheelCtrl.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			bSpeedCtrl.checked=FALSE;
			return TRUE;
		}
	}
	else if(id==sLin.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			tbLin.text=format.toString(remap(sLin.getRatio(),0.0,1.0,-1.0,1.0));
			return TRUE;
		}
	}
	else if(id==sRot.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			tbRot.text=format.toString(remap(sRot.getRatio(),0.0,1.0,1.0,-1.0));
			return TRUE;
		}
	}
	else if(id==tbLin.id)
	{
		if(code==FLAT_ACTION_EDIT)
		{
			double r=remap(limit(format.toDouble(tbLin.getText()),-1.0,1.0),-1.0,1.0,0.0,1.0);
			sLin.setRatio(r);
			return TRUE;
		}
	}
	else if(id==tbRot.id)
	{
		if(code==FLAT_ACTION_EDIT)
		{
			double r=remap(limit(format.toDouble(tbRot.getText()),-1.0,1.0),1.0,-1.0,0.0,1.0);
			sRot.setRatio(r);
			return TRUE;
		}
	}
	else if(id==tbDistCritic.id)
	{
		if(code==FLAT_ACTION_EDIT)
		{
			distCritic=(float)limit(format.toDouble(tbDistCritic.getText()),MIN_DIST,MAX_DIST);
			return TRUE;
		}
	}
	else if(id==tbDistStop.id)
	{
		if(code==FLAT_ACTION_EDIT)
		{
			distStop=(float)limit(format.toDouble(tbDistStop.getText()),MIN_DIST,MAX_DIST);
			return TRUE;
		}
	}
	return FALSE;
}
void WifibotControlPanel::draw2D()
{
	double tmp;
	font.color=INT_BLACK;
	font.x=10;
	font.y=TEXT_START_Y;
	glPrint(&font,_T("RUNTIME:     %.3lf s"),time);
	font.y+=18;
	glPrint(&font,_T("PERIOD:      %.3lf ms"),1000*rcvbuf.period);
	font.y+=18;
	glPrint(&font,_T("WAITING:     %d"),robot.getWaitingCount());
	font.color=INT_BLACK;
	font.y+=18;
	tmp=robotSensors.BatVoltage;
	glPrint(&font,_T("BAT VOLTAGE: %d %%"),round(100*tmp));
	drawLineIndicator(
			font.x+200.0f,font.y-10.0f,100.0f,10.0f,
			(float)tmp,2.0f,DIR_RIGHT,
			INT_RED,INT_YELLOW,INT_GREEN,
			INT_WHITE,INT_BLACK);
	font.y+=18;
	glPrint(&font,_T("CTRL WORD:   %02X%02X"),coml,comr);
	font.y+=18;
	glPrint(&font,_T("LEFT: %+.4lf, RIGHT: %+.4lf"),sl,sr);
	font.y+=18;
	glPrint(&font,_T("LIN:  %+.4lf, ROT:   %+.4lf"),vlin,vrot);
	font.y+=18;
	glPrint(&font,_T("AVOID:%+.4lf, ATTEN: %+.4lf"),kav,kat);
	
	font.x=350;
	font.y=TEXT_START_Y;
	font.y+=18;
	glPrint(&font,_T("FRONT LEFT:  %5.1f rpm"),robotSensors.SpeedFrontLeft);
	font.y+=18;
	glPrint(&font,_T("FRONT RIGHT: %5.1f rpm"),robotSensors.SpeedFrontRight);
	font.y+=18;
	glPrint(&font,_T("REAR LEFT:   %5.1f rpm"),robotSensors.SpeedRearLeft);
	font.y+=18;
	glPrint(&font,_T("REAR RIGHT:  %5.1f rpm"),robotSensors.SpeedRearRight);
	font.y+=18;
	glPrint(&font,_T("LEFT RANGE:  %03d (%05.1f cm)"),int(robotSensors.IRLeft),100*robotSensors.DistLeft);
	/*
	drawLineIndicator(
			font.x+300,font.y-10,100,10,
			tmp,2.0f,DIR_RIGHT,
			INT_RED,INT_YELLOW,INT_GREEN,
			INT_WHITE,INT_BLACK);*/
	font.y+=18;
	glPrint(&font,_T("RIGHT RANGE: %03d (%05.1f cm)"),int(robotSensors.IRRight),100*robotSensors.DistRight);
	/*
	drawLineIndicator(
			font.x+300,font.y-10,100,10,
			tmp,2.0f,DIR_RIGHT,
			INT_RED,INT_YELLOW,INT_GREEN,
			INT_WHITE,INT_BLACK);*/
}

} // namespace Lio

int WINAPI _tWinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nCmdShow)
{
	return Lio::app.main(hInstance,hPrevInstance,lpCmdLine,nCmdShow);
}
